# 通过串口导入夹爪控制

import serial#导入串口通信库
from time import sleep
import rospy
from std_msgs.msg import Int8

# grap为设置正向 loose为设置反向 每发送一次 均为单次执行 
# 若夹红枣 只需单次 杯子需要双次 
gripper_grap = b'\x01\x06\x00\x0b\x00\x01\x39\xc8'
gripper_loose = b'\x01\x06\x00\x0b\x00\x00\xf8\x08'
dotask = b'\x01\x05\x00\x07\xff\x00\x3d\xfb'

def send(send_data):
    '''
    该函数已经包含执行命令
    :send_data  send_data可为gripper_grap或者 gripper_loose 
    '''
    ser = serial.Serial('/dev/ttyUSB0',9600,timeout=0.5)
    if(ser.isOpen()):
        result = ser.write(send_data)#编码 send_data.encode('utf-8')
        print("发送成功",result,send_data)
        sleep(0.5)
        ser.write(dotask)
    else:
        print("发送失败！")
    ser.close()


# rospy.init_node('gripper_publisher')
pub = rospy.Publisher('gripper_control', Int8,queue_size=10)
sleep(2)
def newcontrol(msg):
    '''0 张开 1闭合 2中间状态  int'''
    pub.publish(msg)


